#ifndef IMUTYPES_H
#define IMUTYPES_H

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <sophus/se3.hpp>

#include <vector>
#include <mutex>

#include <opencv2/core/core.hpp>

#include <boost/serialization/serialization.hpp>
#include <boost/serialization/vector.hpp>

#include "./Filter/State.h"
#include "Converter.h"

//using namespace cv;
namespace msf
{

namespace IMU
{

const float GRAVITY_VALUE=9.7921;//9.7940

// IMU测量值(陀螺仪，加速度和时间戳)
class Point
{
public:
    Point(const float &acc_x, const float &acc_y, const float &acc_z,
             const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
             const double &timestamp): a(acc_x,acc_y,acc_z), w(ang_vel_x,ang_vel_y,ang_vel_z), t(timestamp){}
    Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp):
        a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){}
    // copy constructor to copy data from another Point object
    Point(const Point &obj){
        a = obj.a;
        w = obj.w;
        t = obj.t;
    }
public:
    Eigen::Vector3f a;
    Eigen::Vector3f w;
    double t;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//using ImuDataPtr = std::shared_ptr<Point>;
//using ImuDataConstPtr = std::shared_ptr<const Point>;

// IMU偏差(陀螺仪和加速度):bias固有的，可静态条件下标定
class Bias
{
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive & ar, const unsigned int version) // 序列化的函数，它的作用是把Bias类的实例转换成二进制数据流或XML格式，以便保存或传输
      {
            ar & bax;
            ar & bay;
            ar & baz;

            ar & bwx;
            ar & bwy;
            ar & bwz;
      }

public:
      Bias():bax(0),bay(0),baz(0),bwx(0),bwy(0),bwz(0){}
      Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
                  const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z):
                  bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z){}
      void CopyFrom(Bias &b);
      friend std::ostream& operator<< (std::ostream &out, const Bias &b);// 让这个函数可以访问类Bias的私有成员，从而可以把Bias对象的信息输出到std::ostream对象中
                                                                         // std::cout << b << std::endl; //调用友元函数operator<<，把b的信息输出到std::cout中
public:
      float bax, bay, baz;
      float bwx, bwy, bwz;
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

};

//IMU标定 (Tbo, Tob, noise):noise是动态条件下的
class Calib
{
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
        serializeSophusSE3(ar,mTob,version);
        serializeSophusSE3(ar,mTbo,version);

        ar & boost::serialization::make_array(Cov.diagonal().data(), Cov.diagonal().size());
        ar & boost::serialization::make_array(CovWalk.diagonal().data(), CovWalk.diagonal().size());

        ar & mbIsSet;
    }

public:

    Calib(const Sophus::SE3<float> &Tbo, const float &ng, const float &na, const float &ngw, const float &naw)
    {
        Set(Tbo,ng,na,ngw,naw);
    }

    Calib(const Calib &calib);
    Calib(){mbIsSet = false;}

    //void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const float &ngw, const float &naw);
    void Set(const Sophus::SE3<float> &sophTbo, const float &ng, const float &na, const float &ngw, const float &naw);

public:
    // Sophus/Eigen implementation
    Sophus::SE3<float> mTob;
    Sophus::SE3<float> mTbo;
    Eigen::DiagonalMatrix<float,6> Cov, CovWalk;
    bool mbIsSet;
};

// 一个陀螺仪测量值的积分
class IntegratedRotation
{
public:
    IntegratedRotation(){}
    IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time);

public:
    float deltaT; //integration time
    Eigen::Matrix3f deltaR;
    Eigen::Matrix3f rightJ; // right jacobian
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

// IMU测量值的预积分
class Preintegrated
{
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive & ar, const unsigned int version)
      {
            ar & dT;
            ar & boost::serialization::make_array(C.data(), C.size());
            ar & boost::serialization::make_array(Info.data(), Info.size());
            ar & boost::serialization::make_array(Nga.diagonal().data(), Nga.diagonal().size());
            ar & boost::serialization::make_array(NgaWalk.diagonal().data(), NgaWalk.diagonal().size());
            ar & b;
            ar & boost::serialization::make_array(dR.data(), dR.size());
            ar & boost::serialization::make_array(dV.data(), dV.size());
            ar & boost::serialization::make_array(dP.data(), dP.size());
            ar & boost::serialization::make_array(JRg.data(), JRg.size());
            ar & boost::serialization::make_array(JVg.data(), JVg.size());
            ar & boost::serialization::make_array(JVa.data(), JVa.size());
            ar & boost::serialization::make_array(JPg.data(), JPg.size());
            ar & boost::serialization::make_array(JPa.data(), JPa.size());
            ar & boost::serialization::make_array(avgA.data(), avgA.size());
            ar & boost::serialization::make_array(avgW.data(), avgW.size());

            ar & bu;
            ar & boost::serialization::make_array(db.data(), db.size());
            ar & mvMeasurements;
      }
public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
      Preintegrated(const Bias &b_, const Calib &calib);
      Preintegrated(Preintegrated* pImuPre);
      Preintegrated() {}
      ~Preintegrated() {}
      void CopyFrom(Preintegrated* pImuPre);
      void Initialize(const Bias &b_);
      void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt);
      void Reintegrate();
      void MergePrevious(Preintegrated* pPrev);
      void SetNewBias(const Bias &bu_);
      IMU::Bias GetDeltaBias(const Bias &b_);

      Eigen::Matrix3f GetDeltaRotation(const Bias &b_);
      Eigen::Vector3f GetDeltaVelocity(const Bias &b_);
      Eigen::Vector3f GetDeltaPosition(const Bias &b_);

      Eigen::Matrix3f GetUpdatedDeltaRotation();
      Eigen::Vector3f GetUpdatedDeltaVelocity();
      Eigen::Vector3f GetUpdatedDeltaPosition();

      Eigen::Matrix3f GetOriginalDeltaRotation();
      Eigen::Vector3f GetOriginalDeltaVelocity();
      Eigen::Vector3f GetOriginalDeltaPosition();

      Eigen::Matrix<float,6,1> GetDeltaBias();

      Bias GetOriginalBias();
      Bias GetUpdatedBias();

      void printMeasurements() const {
            std::cout << "pint meas:\n";
            for(int i=0; i<mvMeasurements.size(); i++){
                  std::cout << "meas " << mvMeasurements[i].t << std::endl;
            }
            std::cout << "end pint meas:\n";
      }

public:
      float dT;
      Eigen::Matrix<float,15,15> C;
      Eigen::Matrix<float,15,15> Info;
      Eigen::DiagonalMatrix<float,6> Nga, NgaWalk;

      // Values for the original bias (when integration was computed)
      Bias b;
      Eigen::Matrix3f dR;
      Eigen::Vector3f dV, dP;
      Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa;
      Eigen::Vector3f avgA, avgW;

private:
    //更新后的零偏
    Bias bu;    
    // 初始偏置和更新后偏置的差
    // 用于计算更新后的预积分值 
    Eigen::Matrix<float,6,1> db;

    struct integrable
    {
        template<class Archive>
        void serialize(Archive & ar, const unsigned int version)
        {
            ar & boost::serialization::make_array(a.data(), a.size());
            ar & boost::serialization::make_array(w.data(), w.size());
            ar & t;
        }

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        integrable(){}
        integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_ , const float &t_):a(a_),w(w_),t(t_){}
        Eigen::Vector3f a, w;
        float t;
    };

    std::vector<integrable> mvMeasurements;

    std::mutex mMutex;
};

// 李代数函数
Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z);
Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v);

Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y, const float &z);
Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v);

Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R);

// ESKF
/**
   * @brief
   *
   * @ref ESKF 5.4.1 The nominal state kinematics (without noise)
   *
   * @param last_imu
   * @param curr_imu
   * @param last_state
   * @param state
   * @param vec_na
   * @param vec_ng
   * @param vec_wa
   * @param vec_wg
   */
void propagate_state(
    const IMU::Point last_imu,
    const IMU::Point curr_imu,
    const State &last_state,
    State &state,
    const Calib &calib,
    bool with_noise = false,
    bool with_const_noise = true,
    const Eigen::Matrix<double, kNoiseDim, 1> &vec_noise = Eigen::Matrix<double, kNoiseDim, 1>::Zero());

/**
   * @brief
   *
   * @ref ESKF
   *      5.4.3 (local angular error)
   *      7.2.3 (global angular error)
   *      The error-state Jacobian and perturbation matrices
   *
   * @param last_imu
   * @param curr_imu
   * @param last_state
   * @param state
   */
  void propagate_state_cov(const IMU::Point last_imu, const IMU::Point curr_imu, const State &last_state, State &state, const Calib &calib);

// 噪声协方差矩阵 Qi
Eigen::Matrix<double, kNoiseDim, kNoiseDim> noise_cov(double dt, const Calib &calib);

// 离散时间下的噪声协方差矩阵 Fi*Qi*Fi.t
Eigen::Matrix<double, kStateDim, kStateDim> noise_cov_discret_time(double dt, const Calib &calib);

}

} // namespace msf


#endif //IMUTYPES_H